#include "MinNormProblem.h"
#include "GLPKInterface.h"
#include "QPActiveSetSolver.h"
#include "LSQRInterface.h"
#include <math/linalgebra.h>
#include <errors.h>
using namespace Optimization;
using namespace std;

MinNormProblem::MinNormProblem()
  :norm(Inf),verbose(0)
{}

bool MinNormProblem::IsValid() const
{
  if(norm != One && norm != Two && !IsInf(norm)) {
    cerr<<"MinNormProblem::IsValid(): Invalid norm"<<endl;
    return false;
  }
  if(C.isEmpty()) {
    cerr<<"MinNormProblem::IsValid(): Empty problem"<<endl;
    return false;
  }
  if(C.m != d.n) {
    cerr<<"MinNormProblem::IsValid(): C.m != d.n"<<endl;
    return false;
  }
  if(!LinearConstraints::IsValid()) {
    cerr<<"MinNormProblem::IsValid(): Constraints not valid"<<endl;
    printf("A(%d x %d) p(%d) q(%d) l(%d) u(%d)\n",A.m,A.n,p.n,q.n,l.n,u.n);
    return false;
  }
  if(!A.isEmpty() && C.n != A.n) {
    cerr<<"MinNormProblem::IsValid(): Constraint size does not match objective size"<<endl;
    return false;
  }
  return true;
}

void MinNormProblem::Print(std::ostream& out) const
{
  out<<"min L"<<norm<<" norm of: "<<endl;
  for(int i=0;i<C.m;i++) {
    Vector ci;
    C.getRowRef(i,ci);
    out<<"["<<VectorPrinter(ci)<<"].x - "<<d(i)<<endl;
  }
  out<<"w.r.t. x";
  if(!A.isEmpty()) {
    out<<" such that "<<endl;
    LinearConstraints::Print(out);
  }
}

void MinNormProblem::Assemble()
{
  //in the below comments, let 1 be the vector of all 1's
  if(norm == One) {
    /* Convert to 
      min_{x,e} 1te
      b <= Cx + Ie
      Cx - Ie <= d
    */
    lp.Resize(C.m*2+A.m,C.n+C.m);
    lp.minimize = true;
    for(int i=0;i<C.m;i++) lp.c(C.n+i) = One;
    lp.A.copySubMatrix(0,0,C);
    for(int i=0;i<C.m;i++) lp.A(i,i+C.n) = 1;
    lp.q.copySubVector(0,d);
    lp.A.copySubMatrix(C.m,0,C);
    for(int i=0;i<C.m;i++) lp.A(i+C.m,i+C.n) = -1;
    lp.p.copySubVector(C.m,d);

    if(A.m != 0) {
      lp.A.copySubMatrix(C.m*2,0,A);
      lp.q.copySubVector(C.m*2,q);
      lp.p.copySubVector(C.m*2,p);
    }
    if(!l.isEmpty()) 
      lp.l.copySubVector(0,l);
    if(!u.isEmpty()) 
      lp.u.copySubVector(0,u);
  }
  else if(norm == Two) {
    if(lp.HasInequalities()) {
      qp.Pobj.mulTransposeA(C,C);
      A.mulTranspose(d,qp.qobj); qp.qobj.inplaceNegative();
      
      qp.SetRef(*this);  //sets up the linear constraints
    }
    else {  //no need to use quadratic program if no inequality constraints
    }
  }
  else {
    Assert(IsInf(norm));
    /* Convert to 
      min_{x,d} d
      b <= Cx + 1d
      Cx - 1d <= b
    */
    lp.Resize(C.m*2+A.m,C.n+1);
    lp.minimize = true;
    lp.c.setZero();
    lp.c(C.n) = 1;
    lp.A.copySubMatrix(0,0,C);
    for(int i=0;i<C.m;i++) lp.A(i,C.n) = 1;
    lp.q.copySubVector(0,d);
    lp.A.copySubMatrix(C.m,0,C);
    for(int i=0;i<C.m;i++) lp.A(C.m+i,C.n) = -1;
    lp.p.copySubVector(C.m,d);

    if(!A.isEmpty()) {
      lp.A.copySubMatrix(C.m*2,0,A);
      lp.q.copySubVector(C.m*2,q);
      lp.p.copySubVector(C.m*2,p);
    }
    if(!l.isEmpty()) 
      lp.l.copySubVector(0,l);
    if(!u.isEmpty()) 
      lp.u.copySubVector(0,u);
  }
}

Real MinNormProblem::Norm(const Vector& x) const
{
  Vector temp;
  C.mul(x,temp);
  temp -= d;
  if(norm==One) {
    Real sum=0;
    for(int i=0;i<temp.n;i++)
      sum += Abs(temp(i));
    return sum;
  }
  else if(norm==Two) {
    return temp.norm();
  }
  else {
    Assert(IsInf(norm));
    return temp.maxAbsElement();
  }
}

LinearProgram::Result MinNormProblem::Solve(Vector& x) 
{
  if(norm == 2) {
    Assert(IsValid());

    if(HasInequalities()) {  //must use quadratic program for inequalities
      Assert(qp.Pobj.m == C.m);
      Assert(qp.A.isRef());
      QPActiveSetSolver solver(qp);
      solver.verbose = verbose;
      ConvergenceResult res=solver.Solve();
      if(res == ConvergenceError) {
	if(verbose >= 1)
	  cerr<<"Quadratic program unable to solve constrained least-squares problem"<<endl;
	getchar();
	return LinearProgram::Infeasible;
      }
      else if(res == MaxItersReached) {
	x = solver.x;
	return LinearProgram::Error;
      }
      else {
	x = solver.x;
	return LinearProgram::Feasible;
      }
    }
    else if(A.m != 0) {  //no inequality constraints, just equality
      //just transform the problem to an equivalent one
      //Ax=p => x = A#*p + N*y = x0 + N*y
      //where A# is pseudoinverse, N is nullspace
      //so lsq problem becomes min |C*N*y - (d-C*x0)|
      Assert(A.m <= A.n);  //otherwise overconstrained
      
      Matrix C_new, N;
      Vector d_new, x0, y;
      
      MatrixEquation eq(A,p);
      if(!eq.AllSolutions(x0,N)) {
	if(verbose >= 1)
	  cerr<<"MinNormProblem (norm 2): Error solving for all solutions to equality constraints"<<endl;
	if(verbose >= 2) {
	  cerr<<"Press any key to continue"<<endl;
	  getchar();
	}
	return LinearProgram::Error;
      }
      if(verbose >= 2) {
	Vector r;
	eq.Residual(x0,r);
	if(r.norm() > 1e-4) {
	  cout<<"Residual of Aeq*x0=beq: "<<VectorPrinter(r)<<endl;
	  cout<<"Norm is "<<r.norm()<<endl;
	  if(r.norm() > 1e-2) {
	    cout<<MatrixPrinter(A)<<endl;
	    cout<<"Press any key to continue"<<endl;
	    getchar();
	    return LinearProgram::Error;
	  }
	  cout<<"Press any key to continue"<<endl;
	  getchar();
	}
      }
      
      if(verbose >= 1) {
	cout<<"Projecting problem on equality constraints"<<endl;
	cout<<"Original dimension "<<A.n<<", nullspace dimension "<<N.n<<endl;
      }
      
      //set bnew
      C.mul(x0,d_new); d_new-=d; d_new.inplaceNegative();
      
      //set Cnew
      C_new.mul(C,N);
      
      if(verbose >= 2) {
	cout<<"x0: "<<VectorPrinter(x0)<<endl;
	cout<<"N: "<<endl<<MatrixPrinter(N)<<endl;
      }
      if(verbose >=1) cout<<"Solving transformed problem..."<<endl;
      
      MatrixEquation ls(C_new,d_new);
      if(!ls.LeastSquares(y)) {
	cerr<<"LeastSquares: Error solving transformed least squares!!!"<<endl;
	if(verbose >=1) {
	  cerr<<"Press any key to continue"<<endl;
	  getchar();
	}
	return LinearProgram::Error;
      }
      //go back to x
      x = x0;
      N.madd(y,x);
      return LinearProgram::Feasible;
    }
    else {
      MatrixEquation ls(C,d);
      if(!ls.LeastSquares(x)) {
	cout<<"Error solving for least squares!!!"<<endl;
	return LinearProgram::Error;
      }
      return LinearProgram::Feasible;
    }
  }
  else {
    Vector xopt;
    GLPKInterface glpk;
    glpk.Set(lp);
    LinearProgram::Result res=glpk.Solve(xopt);
    if(res==LinearProgram::Feasible) {
      x.resize(C.n);
      xopt.getSubVectorCopy(0,x);
    }
    return res;
  }
}
  





MinNormProblem_Sparse::MinNormProblem_Sparse()
  :norm(Inf),verbose(0)
{}

bool MinNormProblem_Sparse::IsValid() const
{
  if(norm != One && norm != Two && !IsInf(norm)) {
    cerr<<"MinNormProblem_Sparse::IsValid(): Invalid norm"<<endl;
    return false;
  }
  if(C.isEmpty()) {
    cerr<<"MinNormProblem_Sparse::IsValid(): Empty problem"<<endl;
    return false;
  }
  if(C.m != d.n) {
    cerr<<"MinNormProblem_Sparse::IsValid(): C.m != d.n"<<endl;
    return false;
  }
  if(!LinearConstraints_Sparse::IsValid()) {
    cerr<<"MinNormProblem_Sparse::IsValid(): Constraints not valid"<<endl;
    return false;
  }
  if(!A.isEmpty() && C.n != A.n) {
    cerr<<"MinNormProblem_Sparse::IsValid(): Constraint size does not match objective size"<<endl;
    return false;
  }
  return true;
}

void MinNormProblem_Sparse::Print(std::ostream& out) const
{
  out<<"min L"<<norm<<" norm of: "<<endl;
  for(int i=0;i<C.m;i++) {
    SparseMatrix::ConstRowIterator j=C.rows[i].begin();
    out<<j->second<<"*"<<"x["<<j->first<<"]";
    j++;
    for(;j!=C.rows[i].end();j++) {
      if(j->second > 0)
	out<<"+"<<j->second<<"*x["<<j->first<<"]";
      else if(j->second < 0) 
	out<<"-"<<-j->second<<"*x["<<j->first<<"]";
    }
    out<<" - "<<d(i)<<endl;
  }
  out<<"w.r.t. x";
  if(!A.isEmpty()) {
    out<<" such that "<<endl;
    LinearConstraints_Sparse::Print(out);
  }
}

void MinNormProblem_Sparse::Assemble()
{
  //in the below comments, let 1 be the vector of all 1's
  if(norm == One) {
    /* Convert to 
      min_{x,e} 1te
      b <= Cx + Ie
      Cx - Ie <= d
    */
    lp.Resize(C.m*2+A.m,C.n+C.m);
    lp.minimize = true;
    for(int i=0;i<C.m;i++) lp.c(C.n+i) = One;
    lp.A.copySubMatrix(0,0,C);
    for(int i=0;i<C.m;i++) lp.A(i,i+C.n) = 1;
    lp.q.copySubVector(0,d);
    lp.A.copySubMatrix(C.m,0,C);
    for(int i=0;i<C.m;i++) lp.A(i+C.m,i+C.n) = -1;
    lp.p.copySubVector(C.m,d);

    if(!A.isEmpty()) {
      lp.A.copySubMatrix(C.m*2,0,A);
      lp.q.copySubVector(C.m*2,q);
      lp.p.copySubVector(C.m*2,p);
    }
    if(!l.isEmpty()) 
      lp.l.copySubVector(0,l);
    if(!u.isEmpty()) 
      lp.u.copySubVector(0,u);
  }
  else if(norm == Two) {
    if(lp.HasInequalities()) {
      FatalError("Not done with sparse QP");
    }
    else {  //no need to use quadratic program if no inequality constraints
    }
  }
  else {
    Assert(IsInf(norm));
    /* Convert to 
      min_{x,d} d
      b <= Cx + 1d
      Cx - 1d <= b
    */
    lp.Resize(C.m*2+A.m,C.n+1);
    lp.c.setZero();
    lp.c(C.n) = 1;
    lp.A.copySubMatrix(0,0,C);
    for(int i=0;i<C.m;i++) lp.A(i,C.n) = 1;
    lp.q.copySubVector(0,d);
    lp.A.copySubMatrix(C.m,0,C);
    for(int i=0;i<C.m;i++) lp.A(C.m+i,C.n) = -1;
    lp.p.copySubVector(C.m,d);

    if(!A.isEmpty()) {
      lp.A.copySubMatrix(C.m*2,0,A);
      lp.q.copySubVector(C.m*2,q);
      lp.p.copySubVector(C.m*2,p);
    }
    if(!l.isEmpty()) 
      lp.l.copySubVector(0,l);
    if(!u.isEmpty()) 
      lp.u.copySubVector(0,u);
  }
}

Real MinNormProblem_Sparse::Norm(const Vector& x) const
{
  Vector temp;
  C.mul(x,temp);
  temp -= d;
  if(norm==One) {
    Real sum=0;
    for(int i=0;i<temp.n;i++)
      sum += Abs(temp(i));
    return sum;
  }
  else if(norm==Two) {
    return temp.norm();
  }
  else {
    Assert(IsInf(norm));
    return temp.maxAbsElement();
  }
}

LinearProgram::Result MinNormProblem_Sparse::Solve(Vector& x) 
{
  if(norm == 2) {
    Assert(IsValid());

    if(HasInequalities()) {  //must use quadratic program for inequalities
      FatalError("Sparse QP not done yet");
      return LinearProgram::Error;
    }
    else if(A.m != 0) {  //no inequality constraints, just equality
      FatalError("Sparse LS with equality constraints not done yet");
      return LinearProgram::Error;
    }
    else {
      LSQRInterface lsqr;
      if(!lsqr.Solve(C,d)) {
	cout<<"Error solving for least squares!!!"<<endl;
	return LinearProgram::Error;
      }
      x = lsqr.x;
      return LinearProgram::Feasible;
    }
  }
  else {
    Vector xopt;
    GLPKInterface glpk;
    glpk.Set(lp);
    LinearProgram::Result res= glpk.Solve(xopt);
    if(res==LinearProgram::Feasible) {
      x.resize(C.n);
      xopt.getSubVectorCopy(0,x);
    }
    return res;
  }
}
  
